#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>

class ImagePublisherNode : public rclcpp::Node {
public:
    ImagePublisherNode() : Node("image_publisher_pkg_node") {
        publisher_ = this->create_publisher<sensor_msgs::msg::Image>("camera/image", 10);
        
        // 打开摄像头并设置分辨率
        cap_.open(0);
        if (!cap_.isOpened()) {
            RCLCPP_ERROR(this->get_logger(), "Could not open USB camera!");
            return;
        }
        
        cap_.set(cv::CAP_PROP_FRAME_WIDTH, 1920);
        cap_.set(cv::CAP_PROP_FRAME_HEIGHT, 1080);
        
        timer_ = this->create_wall_timer(
            std::chrono::milliseconds(30),
            std::bind(&ImagePublisherNode::timer_callback, this));
    }

    ~ImagePublisherNode() {
        if (cap_.isOpened()) {
            cap_.release();
        }
        cv::destroyAllWindows();
    }

private:
    void timer_callback() {
        cv::Mat frame;
        cap_ >> frame;

        if (frame.empty() || frame.cols <= 0 || frame.rows <= 0 || 
            frame.cols > 3840 || frame.rows > 2160) {
            RCLCPP_WARN(this->get_logger(), "Invalid frame captured");
            return;
        }

        try {
            auto msg = cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", frame).toImageMsg();
            msg->header.stamp = this->now();
            msg->header.frame_id = "camera";
            publisher_->publish(*msg);
            
            cv::imshow("Camera Feed", frame);
            cv::waitKey(1);
        } catch (const cv::Exception& e) {
            RCLCPP_ERROR(this->get_logger(), "OpenCV error: %s", e.what());
        }
    }

    cv::VideoCapture cap_;
    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr publisher_;
};

int main(int argc, char *argv[]) {
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<ImagePublisherNode>());
    rclcpp::shutdown();
    return 0;
}
